/**
 * Copyright (c) suzuki ryoutoku
 * See this code regarding the Apache License Version 2.0
 * @author suzuki ryoutoku
 */

//=============================================================================
//	Protected multiple include
//=============================================================================

#ifndef MATHEMATICS_H
#define MATHEMATICS_H

//=============================================================================
//	pre processor
//=============================================================================

#include "include/declare.h"
using namespace library::mathematics;
#include "math.h"

//=============================================================================
//	Define const value
//=============================================================================
/**
 * tracball's size
 * @author suzuki ryoutoku
 * @date 2007-10-2
 * @version 1.0
 */
#define TRACK_BALL_SIZE 0.8

//=============================================================================
//	Define class
//=============================================================================
/**
 * color
 * \note
 * This class created in order to set RGB value float.
 * @version 1.0
 * @author suzuki ryoutoku
 * @date 2007-06-05
 */
template <typename TYPE> class library::mathematics::Color
{
  // opened member
public:
  union{
	TYPE color[4];
	struct{
	  TYPE r; //!< red
	  TYPE g; //!< green
	  TYPE b; //!< blue
	  TYPE a; //!< alpha blending rate
	};
  };
  
  // opened method
public:
  Color();
  Color(TYPE ir , TYPE ig , TYPE ib , TYPE ia );
  Color(unsigned int arg);
  Color(unsigned char ir, unsigned char ig, unsigned char ib);
  
  Color(const Color&);
};

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE> Color<TYPE>::Color()
{
  this->a = this->b =this->g=this->r = 0;
}

/**
 * @param ir substituted 0.0 in default 
 * @param ig substituted 0.0 in default 
 * @param ib substituted 0.0 in default 
 * @param ia substituted 0.0 in default 
 * @version 1.0
 * @author suzuki ryoutoku
 * @date 2007-06-05
 */
template <typename TYPE> Color<TYPE>::Color(TYPE ir ,
											TYPE ig , 
											TYPE ib , 
											TYPE ia )
{
    this->r = ir;
    this->g = ig;
    this->b = ib;
    this->a = ia;
}

/**
 * @author suzuki ryoutoku
 * @date 2007-11-13
 * @version 1.0
 */
template <typename TYPE> Color<TYPE>::Color(unsigned int arg)
{
  this->r = ((TYPE)((arg    ) & 0xFF)/255.0);
  this->g = ((TYPE)((arg>> 8) & 0xFF)/255.0);
  this->b = ((TYPE)((arg>>16) & 0xFF)/255.0);
  //  this->a = (TYPE)((arg>>24) & 0xFF);
  this->a = (TYPE)1.0;
}

/**
 * copy construct
 * @param arg 
 * @version 1.0
 * @author suzuki ryoutoku
 * @date 2007-06-05
 */
template <typename TYPE> Color<TYPE>::Color(const Color& arg)
{
    this->r = arg.r;
    this->g = arg.g;
    this->b = arg.b;
    this->a = arg.a;
}

/**
 *
 * \note
 *
 *
 * @author suzuki ryoutoku
 * @version 1.0
 * @date 2008-2-23
 */
template <typename TYPE> Color<TYPE>::Color(unsigned char ir ,
											unsigned char ig , 
											unsigned char ib)
{
    this->r = ((TYPE)ir)/255.0;
    this->g = ((TYPE)ig)/255.0;
    this->b = ((TYPE)ib)/255.0;
    this->a = (TYPE)1;
}


/**
 * vector in 3dcg mathematics
 * \note
 * Dimention is homogeneous coordinates (4 component)
 * @version 1.0
 * @author suzuki ryoutoku
 * @date 2007-06-13
 */
template <typename TYPE> class library::mathematics::Vector
{
  // opened member
public:
    union{
	  TYPE vec[4];
	  struct{
		TYPE x, y, z, w;
	  };
    };
  
  // opened method
public:
  Vector();
  Vector(TYPE ix, TYPE iy, TYPE iz, TYPE iw);
  Vector(const Vector&);

  Vector& operator += ( const Vector& );
  Vector& operator -= ( const Vector& );
  Vector& operator *= ( TYPE );
  Vector& operator /= ( TYPE );
  
  // complement
  Vector operator - () const;
  
  Vector operator + ( const Vector& ) const;
  Vector operator - ( const Vector& ) const;
  Vector operator * ( TYPE ) const;
  Vector operator / ( TYPE ) const;
  
  bool operator == ( const Vector& ) const;
  bool operator != ( const Vector& ) const;

  TYPE innerProduct(const Vector& arg);
  TYPE getLength(void);
  Vector normalizing(void);


  Vector outerProduct(const Vector& left);
#if 0    
  bool isZero();
#endif
};

/**
 * Vector in 3dcg mathematics
 * \note
 * Dimention is homogeneous coordinates (3 component)
 * @version 1.0
 * @author suzuki ryoutoku
 * @date 2008-05-23
 */
template <typename TYPE> class library::mathematics::Vector3D
{
  // opened member
public:
    union{
	  TYPE vec[3];
	  struct{
		TYPE x, y, z;
	  };
    };
  
  // opened method
public:
  Vector3D();
  Vector3D(TYPE ix, TYPE iy, TYPE iz);
  Vector3D(const Vector3D&);
  Vector3D(const Vector<TYPE>&);

  Vector3D& operator += ( const Vector3D& );
  Vector3D& operator -= ( const Vector3D& );
  Vector3D& operator *= ( TYPE );
  Vector3D& operator /= ( TYPE );
  
  // complement
  Vector3D operator - () const;
  Vector3D operator + ( const Vector3D& ) const;
  Vector3D operator - ( const Vector3D& ) const;
  Vector3D operator * ( TYPE ) const;
  Vector3D operator / ( TYPE ) const;
  
  bool operator == ( const Vector3D& ) const;
  bool operator != ( const Vector3D& ) const;

  TYPE innerProduct(const Vector3D& arg);
  TYPE getLength(void);
  Vector3D normalizing(void);


  Vector3D outerProduct(const Vector3D& left);
#if 0    
  bool isZero();
#endif
};


/**
 * Matrix in 3dcg mathematics
 * \note
 * It's complements are 16.
 * @author suzuki ryoutoku
 * @date 2007-6-14
 * @version 1.0
 */
template <typename TYPE> class library::mathematics::Matrix
{
  // opened memeber
public:
  union{
	TYPE m[4][4];
	struct
	{
	  TYPE m00, m01, m02, m03;
	  TYPE m04, m05, m06, m07;
	  TYPE m08, m09, m10, m11;
	  TYPE m12, m13, m14, m15;
	  /* open gl
		m00, m04, m08, m12
    	m01, m05, m09, m13
    	m02, m06, m10, m14
    	m03, m07, m11, m15

	  */
	};
  };
  
  // opened mtehod
public:
  Matrix();
  Matrix(const Quaternion<double>&);
  const Vector<double> operator*(const Vector<double>& left);
  void setRotateX(TYPE arg);
  void setRotateY(TYPE arg);
  void setRotateZ(TYPE arg);
};


/**
 * quaternion in 3dcg mathematics
 * @author suzuki ryoutoku
 * @date 2007-9-30
 * @version 1.0
 */
template <typename TYPE> class library::mathematics::Quaternion
{
  // opened memeber
public:
  union{
	TYPE vec[4];
	struct{
	  TYPE x, y, z, w;
	};
  };

  // opened method
public:
  Quaternion();
  Quaternion(TYPE arg1, TYPE arg2, TYPE arg3, TYPE arg4);
  Quaternion(const Quaternion&);
  Quaternion(Vector<double> axis, double angle );
  Quaternion(Vector<float> axis, float angle );
  
  Quaternion& operator *= ( const Quaternion& );
  
  // calss method
public:
  static Quaternion Trackball(TYPE p1x, TYPE p1y, TYPE p2x, TYPE p2y);
  static TYPE ProjectToSphere(TYPE r, TYPE x, TYPE y);
};

/**
 * Vector for uv values.
 * @author suzuki ryoutoku
 * @version 1.0
 * @date 2008-2-3
 */
template <typename TYPE> class library::mathematics::Vector2D
{
  // opened member
public:
    union{
	  TYPE vec[2];
	  struct{
		TYPE u, v;
	  };
    };
  // opened method
public:
  Vector2D();
  Vector2D(TYPE iu, TYPE iv);
  Vector2D(const Vector2D&);

  Vector2D operator *= (const Vector2D& );
};

//=============================================================================
//	Vector's method
//=============================================================================
/**
 * Initialize, create zero vector.
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template<typename TYPE> Vector<TYPE>::Vector()
{
  this->x = this->y = this->z = this->w = 0;
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template<typename TYPE> 
Vector<TYPE>::Vector(TYPE ix, TYPE iy, TYPE iz, TYPE iw)
{
  this->x = ix;
  this->y = iy;
  this->z = iz;
  this->w = iw;
}


/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE> Vector<TYPE>::Vector(const Vector<TYPE>& arg)
{
  this->x = arg.x;
  this->y = arg.y;
  this->z = arg.z;
  this->w = arg.w;
}


/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE> 
Vector<TYPE>& Vector<TYPE>::operator += (const Vector& arg)
{
    this->x += arg.x;
    this->y += arg.y;
    this->z += arg.z;
    this->w += arg.w;
    return *this;  
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE>& Vector<TYPE>::operator -= ( const Vector<TYPE>& arg)
{
  this->x -= arg.x;
  this->y -= arg.y;
  this->z -= arg.z;
  this->w -= arg.w;
  return *this;
}


/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE>& Vector<TYPE>::operator *= ( TYPE arg)
{
  this->x *= arg;
  this->y *= arg;
  this->z *= arg;
  this->w *= arg;
  return *this;
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE>& Vector<TYPE>::operator /= ( TYPE arg)
{
  this->x /= arg;
  this->y /= arg;
  this->z /= arg;
  this->w /= arg;
  
  return *this;
}
  

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE> Vector<TYPE>::operator - () const
{
  return Vector( - this->x,
				 - this->y,
				 - this->z,
				 - this->w );
}

  
/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE> Vector<TYPE>::operator + ( const Vector<TYPE>& arg) const
{
  return Vector( this->x + arg.x,
				 this->y + arg.y,
				 this->z + arg.z,
				 this->w + arg.w );
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE> Vector<TYPE>::operator - ( const Vector<TYPE>& arg) const
{
  return Vector( this->x - arg.x,
				 this->y - arg.y,
				 this->z - arg.z,
				 this->w - arg.w );
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE> Vector<TYPE>::operator * ( TYPE arg) const
{
  return Vector( this->x * arg,
				 this->y * arg,
				 this->z * arg,
				 this->w * arg );
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
Vector<TYPE> Vector<TYPE>::operator / ( TYPE arg) const
{
  return Vector( this->x / arg,
				 this->y / arg,
				 this->z / arg,
				 this->w / arg );
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>  
bool Vector<TYPE>::operator == ( const Vector<TYPE>& arg) const
{
  if( this->x == arg.x ){
	if( this->y == arg.y ){
	  if( this->z == arg.z ){
		if( this->w == arg.w ){
		  return true;
		}
	  }
	}
  }
  return false;
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template <typename TYPE>
bool Vector<TYPE>::operator != ( const Vector<TYPE>& arg) const
{
  if( this->x != arg.x||this->y != arg.y||this->z != arg.z||this->w != arg.w ){
	return true;
  }
  return false;
}

/**
 * computing inner product.
 * \note
 * Computing components are x,y,z.
 * w component is not used.
 *
 * @author suzuki ryoutoku
 * @date 2007-6-25
 * @version 1.0
 */
template <typename TYPE> 
TYPE Vector<TYPE>::innerProduct(const Vector& arg)
{
  TYPE retval = this->x*arg.x + this->y*arg.y + this->z*arg.z;
  return retval;
}


/**
 * Computing outer product.
 * \note
 * w component is not used.
 * @param left Operation object
 * @return Computing result
 *
 * @author suzuki ryoutoku
 * @date 2007-9-29
 * @version 1.0
 */
template <typename TYPE> 
Vector<TYPE> Vector<TYPE>::outerProduct(const Vector<TYPE>& left)
{
  Vector<TYPE> retval;
  retval.x = this->y * left.z - this->z * left.y;
  retval.y = this->z * left.x - this->x * left.z;
  retval.z = this->x * left.y - this->y * left.x;
  return retval;
}


/**
 * return norm
 * \note
 * w component is not use.
 *  
 * @return norm
 * 
 * @author suzuki ryoutoku
 * @date 2007-6-25
 * @version 1.0
 */
template <typename TYPE> 
TYPE Vector<TYPE>::getLength(void)
{
  TYPE retval = fabs(this->x*this->x + this->y*this->y + this->z*this->z);
  return (TYPE)sqrt(retval);
}

/**
 * Computing normalize.
 * \note
 * w component is not used.
 * @author suzuki ryoutoku
 * @date 2007-6-27
 * @version 1.0
 */
template <typename TYPE> 
Vector<TYPE> Vector<TYPE>::normalizing(void)
{
  TYPE length = this->getLength();
  this->x /= length;
  this->y /= length;
  this->z /= length;
  return (*this);
}

//=============================================================================
//	Matrix's method
//=============================================================================
/**
 * \note
 * Diagonal components are 1.0, anything else components are 0.0.
 *
 * @author suzuki ryoutoku
 * @date 2007-6-14
 * @version 1.0
 */
template<typename TYPE> Matrix<TYPE>::Matrix()
{
  for(int i = 0; i < 4; i++){
	for(int j = 0; j < 4; j++){
	  
	  this->m[i][j]= (i==j?1:0);
	}
  }
}


/**
 * Create rotation matrix by quaternion.
 * \note
 * @param arg quaternion
 *
 * @author r-suzuki
 * @date 2007-10-1
 * @version 1.0
 */
template<typename TYPE> Matrix<TYPE>::Matrix(const Quaternion<double>& arg)
{
  this->m[0][0]= 1.0f - 2.0f * (arg.vec[1]*arg.vec[1] + arg.vec[2]*arg.vec[2]);
  this->m[0][1]= 2.0f * (arg.vec[0] * arg.vec[1] - arg.vec[2] * arg.vec[3]);
  this->m[0][2]= 2.0f * (arg.vec[2] * arg.vec[0] + arg.vec[1] * arg.vec[3]);
  this->m[0][3]= 0.0f;
  
  this->m[1][0]= 2.0f * (arg.vec[0] * arg.vec[1] + arg.vec[2] * arg.vec[3]);
  this->m[1][1]= 1.0f - 2.0f * (arg.vec[2]*arg.vec[2] + arg.vec[0]*arg.vec[0]);
  this->m[1][2]= 2.0f * (arg.vec[1] * arg.vec[2] - arg.vec[0] * arg.vec[3]);
  this->m[1][3]= 0.0f;
  
  this->m[2][0]= 2.0f * (arg.vec[2] * arg.vec[0] - arg.vec[1] * arg.vec[3]);
  this->m[2][1]= 2.0f * (arg.vec[1] * arg.vec[2] + arg.vec[0] * arg.vec[3]);
  this->m[2][2]= 1.0f - 2.0f * (arg.vec[1]*arg.vec[1] + arg.vec[0]*arg.vec[0]);
  this->m[2][3]= 0.0f;
  
  this->m[3][0]= 0.0f;
  this->m[3][1]= 0.0f;
  this->m[3][2]= 0.0f;
  this->m[3][3]= 1.0f;

}


/**
 * Transform vector in homogeneous coordinates.
 * @param left Operation object
 * @return vector operated
 * @author suzuki ryoutoku
 * @date 2007-6-22
 * @version 1.0
 */
template <typename TYPE>
const Vector<double> Matrix<TYPE>::operator * (const Vector<double>& left)
{
  Vector<double> retval;
  retval.x = this->m[0][0] * left.x + this->m[1][0] * left.y + this->m[2][0] * left.z + this->m[3][0] * left.w;
  retval.y = this->m[0][1] * left.x + this->m[1][1] * left.y + this->m[2][1] * left.z + this->m[3][1] * left.w;
  retval.z = this->m[0][2] * left.x + this->m[1][2] * left.y + this->m[2][2] * left.z + this->m[3][2] * left.w;
  retval.w = this->m[0][3] * left.x + this->m[1][3] * left.y + this->m[2][3] * left.z + this->m[3][3] * left.w;
  
  return retval;
}

/**
 * rotate matrix in around x axis.
 * @author r-suzuki
 * @date 2007-9-22
 * @version 1.0
 */
template <typename TYPE>
void Matrix<TYPE>::setRotateX(TYPE arg)
{
  this->m[0][0] = 1.0;
  this->m[0][1] = 0.0;
  this->m[0][2] = 0.0;
  this->m[0][3] = 0.0;
  this->m[1][0] = 0.0;
  this->m[1][1] = cos(arg);
  this->m[1][2] = -sin(arg);
  this->m[1][3] = 0.0;
  this->m[2][0] = 0.0;
  this->m[2][1] = sin(arg);
  this->m[2][2] = cos(arg);
  this->m[2][3] = 0.0;
  this->m[3][0] = 0.0;
  this->m[3][1] = 0.0;
  this->m[3][2] = 0.0;
  this->m[3][3] = 1.0;
}

/**
 * rotate matrix in around y axis.
 * @author r-suzuki
 * @date 2007-9-25
 * @version 1.0
 */
template <typename TYPE>
void Matrix<TYPE>::setRotateY(TYPE y)
{
  this->m[0][0] = cos(y);
  this->m[0][1] = 0.0;
  this->m[0][2] = -sinf(y);
  this->m[0][3] = 0.0;
  this->m[1][0] = 0.0;
  this->m[1][1] = 1.0;
  this->m[1][2] = 0.0;
  this->m[1][3] = 0.0;
  this->m[2][0] = sin(y);
  this->m[2][1] = 0.0;
  this->m[2][2] = cos(y);
  this->m[2][3] = 0.0;
  this->m[3][0] = 0.0;
  this->m[3][1] = 0.0;
  this->m[3][2] = 0.0;
  this->m[3][3] = 1.0;
}

/**
 * rotate matrix in around z axis.
 * @author r-suzuki
 * @date 2007-9-25
 * @version 1.0
 */
template <typename TYPE>
void Matrix<TYPE>::setRotateZ(TYPE z)
{
  this->m[0][0] = cos(z);
  this->m[0][1] = sin(z);
  this->m[0][2] = 0.0;
  this->m[0][3] = 0.0;

  this->m[1][0] = -sin(z);
  this->m[1][1] = cos(z);
  this->m[1][2] = 0.0;
  this->m[1][3] = 0.0;

  this->m[2][0] = 0.0;
  this->m[2][1] = 0.0;
  this->m[2][2] = 0.0;
  this->m[2][3] = 0.0;

  this->m[3][0] = 0.0;
  this->m[3][1] = 0.0;
  this->m[3][2] = 0.0;
  this->m[3][3] = 1.0;
}

//=============================================================================
//	Quaternion's method
//=============================================================================
/**
 * x=0,y=0,z=0,w=1
 *
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
Quaternion<TYPE>::Quaternion()
{
  this->x = 0.0;
  this->y = 0.0;
  this->z = 0.0;
  this->w = 1.0;
}


/**
 * @param arg1 Quaternion::x
 * @param arg2 Quaternion::y
 * @param arg3 Quaternion::z
 * @param arg4 Quaternion::w
 *
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
Quaternion<TYPE>::Quaternion(TYPE arg1, TYPE arg2, TYPE arg3, TYPE arg4)
{
  this->x = arg1;
  this->y = arg2;
  this->z = arg3;
  this->w = arg4;
}

/**
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
Quaternion<TYPE>::Quaternion(const Quaternion& arg)
{
  this->x = arg.x;
  this->y = arg.y;
  this->z = arg.z;
  this->w = arg.w;
}

/**
 * Create from axis vector and angle(clockwise)
 * \note
 * @param axis 
 * @param angle 
 *
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
Quaternion<TYPE>::Quaternion(Vector<double> axis, double angle )
{
  axis.normalizing();
  this->x = axis.x * sin(angle * 0.5);
  this->y = axis.y * sin(angle * 0.5);
  this->z = axis.z * sin(angle * 0.5);
  
  this->w = cos(angle/2.0f);
}


/**
 * product quaternion
 * \note
 * @param arg Operated object
 *
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
Quaternion<TYPE>& Quaternion<TYPE>::operator *= (const Quaternion& arg)
{
  Quaternion tmp;
  tmp.vec[0] = this->vec[3]*arg.vec[0] + this->vec[0]*arg.vec[3] + this->vec[1]*arg.vec[2] - this->vec[2]*arg.vec[1];

  tmp.vec[1] = this->vec[3]*arg.vec[1] + this->vec[1]*arg.vec[3] + this->vec[2]*arg.vec[0] - this->vec[0]*arg.vec[2];

  tmp.vec[2] = this->vec[3]*arg.vec[2] + this->vec[2]*arg.vec[3] + this->vec[0]*arg.vec[1] - this->vec[1]*arg.vec[0];
  
  tmp.vec[3] = this->vec[3]*arg.vec[3] - this->vec[0]*arg.vec[0] - this->vec[1]*arg.vec[1] - this->vec[2]*arg.vec[2];

	(*this) = tmp;
  return *this;
}


/**
 * Trackball process
 * \note
 * @param p1x mouse's pre x coordinate
 * @param p1y mouse's pre y coordinate
 * @param p2x mouse's x coordinate
 * @param p2y mouse's y coordinate
 *
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
Quaternion<TYPE> Quaternion<TYPE>::Trackball(TYPE p1x, 
											 TYPE p1y, 
											 TYPE p2x, 
											 TYPE p2y)
{
  Quaternion<TYPE> q;
  if (p1x == p2x && p1y == p2y) {
	q.w = 1.0;
	return q;
  }
  
  
  // transform
  Vector<double> p1 
	= Vector<double>(p1x,p1y,ProjectToSphere(TRACK_BALL_SIZE,p1x,p1y),0.0);
  Vector<double> p2 
	= Vector<double>(p2x,p2y,ProjectToSphere(TRACK_BALL_SIZE,p2x,p2y),0.0);
  
  // outer product
  Vector<double> a = p2.outerProduct(p1);
  
  Vector<double> d = p1 - p2;
  
  double t = d.getLength() / (2.0*TRACK_BALL_SIZE);
  
  
  if( t > 1.0 ){
	t = 1.0;
  }
  if( t < -1.0){
	t = -1.0;
  }


  double phi = 2.0 * asin(t);
    
  q = Quaternion( a, phi);
  return q;
}


/**
 * Project sphere 
 * @author r-suzuki
 * @date 2007-10-2
 * @version 1.0
 */
template <typename TYPE>
TYPE Quaternion<TYPE>::ProjectToSphere(TYPE r, TYPE x, TYPE y)
{
  TYPE d, z;
  
  d = sqrt(x*x + y*y);
  
  if (d < r * 0.70710678118654752440) {    // in sphere
	z = sqrt(r*r - d*d);
  } else {           // above hyperbolic function
	double t = r / 1.41421356237309504880;
	z = t*t / d;
  }
  
  return z;
}

//=============================================================================
//	Vector2D's method
//=============================================================================
/**
 * @author suzuki ryoutoku
 * @version 1.0
 * @date 2008-2-3
 */
template<typename TYPE> Vector2D<TYPE>::Vector2D()
{
  this->u = this->v = (TYPE)0;
}

/**
 * @author suzuki ryoutoku
 * @version 1.0
 * @date 2008-2-3
 */
template<typename TYPE> 
Vector2D<TYPE>::Vector2D(TYPE iu, TYPE iv)
{
  this->u = iu;
  this->v = iv;
}

/**
 * @author suzuki ryoutoku
 * @version 1.0
 * @date 2008-2-3
 */
template <typename TYPE> Vector2D<TYPE>::Vector2D(const Vector2D<TYPE>& arg)
{
  this->u = arg.u;
  this->v = arg.v;
}

/**
 * @author suzuki ryoutoku
 * @version 1.0
 * @date 2008-2-8
 */
template <typename TYPE> 
Vector2D<TYPE> Vector2D<TYPE>::operator *= (const Vector2D<TYPE>& arg)
{
    this->u *= arg.u;
    this->v *= arg.v;
    return *this;  
}

//=============================================================================
//	Vector3D's method
//=============================================================================
/**
 * Initialize, create zero vector.
 * @author suzuki ryoutoku
 * @date 2008-5-23
 * @version 1.0
 */
template<typename TYPE> 
Vector3D<TYPE>::Vector3D()
{
  this->x = this->y = this->z = 0;
}

/**
 * @author suzuki ryoutoku
 * @date 2007-6-13
 * @version 1.0
 */
template<typename TYPE> 
Vector3D<TYPE>::Vector3D(TYPE ix, TYPE iy, TYPE iz)
{
  this->x = ix;
  this->y = iy;
  this->z = iz;
}

/**
 * @author suzuki ryoutoku
 * @date 2008-05-23
 * @version 1.0
 */
template <typename TYPE> 
Vector3D<TYPE>::Vector3D(const Vector3D<TYPE>& arg)
{
  this->x = arg.x;
  this->y = arg.y;
  this->z = arg.z;
}

/**
 * @author suzuki ryoutoku
 * @date 2008-05-23
 * @version 1.0
 */
template <typename TYPE> 
Vector3D<TYPE>::Vector3D(const Vector<TYPE>& arg)
{
  this->x = arg.x;
  this->y = arg.y;
  this->z = arg.z;
}



//=============================================================================
//	Protected multiple include file
//=============================================================================

#endif
